#include "pid_algorithm.h"

unsigned long
simple_discrete_pid_algorithm (struct x_pid_algorithm_params *pparams)
{
    /*
     * u[k] = u[k-1] + k0*e[k] + k1*e[k-1] + k2*e[k-2]
     * k0 = kp + ki + kd
     * k1 = -kp -2*kd
     * k2 = kd
     */

    /* calculate params */
    double k0 = pparams->kp + pparams->ki + pparams->kd;
    double k1 = -(pparams->kp) - 2*(pparams->kd);
    double k2 = pparams->kd;

    /* update error */
    pparams->e[2] = pparams->e[1];
    pparams->e[1] = pparams->e[0];
    pparams->e[0] = pparams->set_point - pparams->current_point;

    /* update output */
    pparams->u[2] = pparams->u[1];
    pparams->u[1] = pparams->u[0];
    /* Calculate pid ouput */
    pparams->u[0] = pparams->u[1] \
                    + k0*pparams->e[0] \
                    + k1*pparams->e[1] \
                    + k2*pparams->e[2];

    if (pparams->u[0] > pparams->max_u) {
        pparams->u[0] = pparams->max_u;
    } else if (pparams->u[0] < pparams->min_u) {
        pparams->u[0] = pparams->min_u;
    } else {
    }
    return (0);
}

unsigned long
discrete_pid_algorithm_type_a (struct x_pid_algorithm_params *pparams)
{
    /*
     * u[k] = p1.u[k-1] + p2.u[k-2] + k0.e[k]+ k1.e[k -1] + k2.e[k- 2]
     * kc = kp
     * ti = kc/ki
     * td = kd/kc
     * ts : sample period
     * gama = td/10
     * p1 = (4*gama)/(ts + 2*gama)
     * p2 = (ts -2*gama)/(ts + 2*gama)
     * k0 = kc*(1 + ts/(2*ti) + 2*td/(ts + 2*gama))
     * k1 = kc*((ts*ts/ti - 4*gama - 4*td)/(ts + 2*gama))
     * k2 = kc*((2*gama - ts + (ts*ts)/(2*ti) - gama*ts/ti + 2*td)/(ts + 2*gama))
     */

    /* calculate params */
    double kc = pparams->kp;
    double ti = kc/pparams->ki;
    double td = pparams->kd/kc;
    unsigned long ts = pparams->sample_period;
    double gama = td/10; /* 10% td */
    double p1 = (4*gama)/(ts + 2*gama);
    double p2 = (ts -2*gama)/(ts + 2*gama);
    double k0 = kc*(1 + ts/(2*ti) + 2*td/(ts + 2*gama));
    double k1 = kc*((ts*ts/ti - 4*gama - 4*td)/(ts + 2*gama));
    double k2 = kc*((2*gama - ts + (ts*ts)/(2*ti) - gama*ts/ti + 2*td)/(ts + 2*gama));

    /* update error */
    pparams->e[2] = pparams->e[1];
    pparams->e[1] = pparams->e[0];
    pparams->e[0] = pparams->set_point - pparams->current_point;

    /* update output */
    pparams->u[2] = pparams->u[1];
    pparams->u[1] = pparams->u[0];
    /* Calculate pid ouput */
    pparams->u[0] = p1*pparams->u[1] \
                    + p2*pparams->u[2] \
                    + k0*pparams->e[0] \
                    + k1*pparams->e[1] \
                    + k2*pparams->e[2];

    if (pparams->u[0] > pparams->max_u) {
        pparams->u[0] = pparams->max_u;
    } else if (pparams->u[0] < pparams->min_u) {
        pparams->u[0] = pparams->min_u;
    } else {
    }
    return (0);
}
unsigned long
classic_discrete_pid_algoritm (struct x_pid_algorithm_params *pparams)
{
    /*
     * u(k) = u(k-1) + kp*(e(k)-e(k-1)) + ki*(e(k)+e(k+1)/2 + kd*(e(k)-2*e(k-1)+e(k-2))
     */

    /* update error */
    pparams->e[2] = pparams->e[1];
    pparams->e[1] = pparams->e[0];
    pparams->e[0] = pparams->set_point - pparams->current_point;

    /* update output */
    pparams->u[2] = pparams->u[1];
    pparams->u[1] = pparams->u[0];
    /* Calculate pid ouput */
    pparams->u[0] = pparams->u[1] \
                    + pparams->kp*(pparams->e[0] - pparams->e[1]) \
                    + pparams->ki*(pparams->e[0] + pparams->e[1])/2 \
                    + pparams->kd*(pparams->e[0] -2*pparams->e[1] + pparams->e[2]);

    if (pparams->u[0] > pparams->max_u) {
        pparams->u[0] = pparams->max_u;
    } else if (pparams->u[0] < pparams->min_u) {
        pparams->u[0] = pparams->min_u;
    } else {
    }
    return (0);
}